/**
*  \file bdi.c
*
*  \brief
*
*  \author Dajin Li  <dajin.li@linde-.de>
*
*
*/

/*===============================================[ private includes  ]================================================*/
//#include "../cmsis/stm32f2xx.h"
//#include "../user/global.h"
//#include "../analoginput/analoginput.h"
//#include "../var/var_tbl.h"
//#include "../icons/icons.h"
//#include "../br115/br115_menu.h"
#include "../parameter/parameter.h"
#include "../nvram/nvram.h"
#include "../app/app.h"
#include "../input/input.h"
#include "../output/output.h"
#include "bdi.h"

/*===============================================[ private defines   ]================================================*/
#define         ChIsZeroThreshold      (5)  // Below this value, detect erroneous runs of zeros
#define         ChIsHighThreshold     (10)  // Above this value was not a zero
#define         Ch100pc            (28800)  // Charge at 100% is assumed to represent 8hrs [in seconds]
#define         ButterRestartGradient (10)  // Butterworth restarting condition gradient (a negative reduction)

// ts = 10s, w0 = 0.0005 rad.s-1, factor = 1 + sqrt(8)/(Ts*w0) + 4/pow(Ts*w0,2) = 160567
#define         CoeffScale1      (1000000)  // used to convert floating point to integer maths
#define         Coeff_uk               (6)  // Butterworth coefficient for u(k)
#define         Coeff_ukm1            (12)  // Butterworth coefficient for u(k-1)
#define         Coeff_ukm2             (6)  // Butterworth coefficient for u(k-2)
#define         CoeffScale2          (100)  // used to convert floating point to integer maths
#define         Coeff_ykm1           (199)  // Butterworth coefficient for y(k-1)
#define         Coeff_ykm2           (-99)  // Butterworth coefficient for y(k-2)

/*===============================================[ private datatypes ]================================================*/

/*===============================================[ private variables ]================================================*/



/*===============================================[ private functions ]================================================*/
static void BDI_Display(void);
static void BDI_Model(void);

/*================================================[ inline functions ]================================================*/
/*===============================================[ public variables ]================================================*/


//----- Global BDI Variables -----
S8 BDI_BattCharge;       // Battery Charge - State Of Charge reading 0 - 100%
S8 BDI_BattCon;          // Battery Condition -  0 - 100%
U16 BDI_VBatt ;          // Coarse Battery Reading allow negative volts scale
S32 Vin;                 // Scaled voltage in reading used in the calculation
U8 BDI_Error;            // error flag - 0 = no error
U8 BDI_LowBattFlag;      // Set when BattCharge < 2%
U8 BDI_BattCutFlag;      // Set when BattCharge < 1%
U8 BDI_FC_Flag;          // Fine/Coarse flag - Set when Fine out of range
U8 BDI_LowBattConFlag;   // Set when BattCon < 1.94V
S32 BattSOC;             // State-Of-Charge modelled voltage
S32 BattCon;             // Battery Condition modelled voltage
S32 Batt_In;             // Rescaled Battery Input voltage for model
S32 last_Batt_In;        // stored for use in BattCon recovery
S32 Vempty ;
S32 vfull_cal;
U8  BDI_run;             // flag to start BDI after initalisation
//--------------------------------
S32 Volts_Stable_cnt;
S32 bdicnt;

//-------- Qmon Stuff --------------
S32  Qmax;               // current max voltage seen in the last 10s
S32  Qmin;               // current min voltage seen in the last 10s
S32  Qmax_next;          // next max voltage seen in the last 10s
S32  Qmin_next;          // next min voltage seen in the last 10s

S32  Qmax_cnt;           // time stamp for current Qmax
S32  Qmin_cnt;           // time stamp for current Qmin
S32  Qmax_next_cnt;      // time stamp for next Qmax
S32  Qmin_next_cnt;      // time stamp for next Qmin
S32  Qmon_cnt;           // Qmon timer
//----------------------------------
U8   xEXTERNAL_BATTERY ;     // previous state of ExtBatt input

//---- Hrs Remaining calculation ----
S32 BDI_HrsRem;             // global for holding Estimation of hours remaining as hours x10 i.e. 86=8.6 hours
S32 xBattCharge;            // local  previous battery charge value
S32 cntBattHrSamples;       // count the number of samples so we know when to start using the values - ignore initial one(s) cos they may be inaccurate
S32 BattTimer;              // timer for timing between % decreases of capacity
U8 BDI_HrsRem_init;         // initialisation flag

U8 BDI_PEDtest;             // test bit to hook into BDI for testing
//S16 BDI_vbatt_cal[3];       // single calibration point
S32 LIDtest[8];             // data buffers for debug/test data outputs on CAN
S32 Qmon ;  // the cell activity in 0.1mV/bit (i.e. 5000=0.5V/cell). This is the difference between the min & max recorded values. 0 = stable voltage - not in use.
//S32 pre_soc;            // last time adc  replace by using stReadWriteParam.pre_soc

//U8 lift_flag2 = 0;        //replace by judging (input_data.seat_switch_1 < 0)
/*================================================[ public functions ]================================================*/
/**
* @brief    Init bdi
* @param    none
* @note     none
* @retval   none
*/
RC bdi_init(void )
{
	RC ret = RC_ERROR;
	
    BDI_HrsRem_init = 0; // PT
    BDI_HrsRem = -1 ;

    BDI_run = 0;
    BDI_PEDtest = 0;

    BDI_BattCharge = -1;
    BDI_BattCon = -1;

    BDI_Error = 0;
    BDI_LowBattFlag = 0;    // assume good volts at startup until BDI_display says otherwise
    BDI_BattCutFlag = 0;    // assume good volts at startup until BDI_display says otherwise
    BDI_LowBattConFlag = 0; // assume low batt condition at startup

    BDI_FC_Flag = 1;        // assume coarse reading
    BDI_VBatt = 0 ;         //-1 NG 27.04.04 for Lint;

    BattSOC = 0;
    BattCon = 0;
    Batt_In = 0;
    last_Batt_In = 0;

    Volts_Stable_cnt = 0;

    bdicnt = 0;

    //----- Initalise Quiescent Monitoring -----
    Qmax = 0;
    Qmin = 25000;
    Qmax_next = 0;
    Qmin_next = 0;

    Qmax_cnt = 0;
    Qmin_cnt = 0;
    Qmax_next_cnt = 0;
    Qmin_next_cnt = 0;
    Qmon_cnt = 0;

	Vempty  = (S32)((service->bdi_cutvpc)*(1.0f+(calibration->bdi_vbatt_cal_min/1000.0f))) * S32_CONST(1000000); // initialise empty value (1.94V/Cell etc) - done before calc every time in case it's changed
	vfull_cal  = (S32)((Vfull)*(1.0f+(calibration->bdi_vbatt_cal_max/1000.0f)));

	ret = sd_read( NVRAM_SD,&stReadWriteParam.pre_soc, &(pReadWriteparam->pre_soc), sizeof(stReadWriteParam.pre_soc) );

    return ret;
} 


/**
* @brief    This task update the fuel capacity
* @param    void const *argument
* @note     none
* @retval   none
*/
void bdi_task(void) 
{
    //Vempty = (S32)service->bdi_cutvpc  * 1000000L;  
    BDI_Model();
    BDI_Display();     // BDI: Displaying on the Console
}  // end task_updt_BDI()   

/*****************************************************************************
        Function:  BDI_Meas
     Description:  Measure the current battery volts
******************************************************************************/
void BDI_Meas_task(void)
{
    BDI_VBatt  = (U32)ADC_VBATT;         // batt volts input scale and single point cal adjust
    //BDI_VBatt  = 5760/2;
    //BDI_VBatt  = 5760;
    Vin =((S32)BDI_VBatt*1000)/240;                           // Vin scaled to give volts-per-cell (20833=2.0833V) for a 48V battery

    //------------------------------------------------------------
    // test stuff
    if(0x1234U == input_data.v_debug_rx_0[0] )
    {
        output_data.v_debug_0[0] = Qmax-Qmin;
        output_data.v_debug_0[1] = Vin;         // Voltage in
        output_data.v_debug_0[2] = BDI_VBatt;   // Voltage in
        output_data.v_debug_0[3] = 0;

        output_data.v_debug_1[0] = Qmax-Qmin;
        output_data.v_debug_1[1] = Vin;                  // Voltage in
        output_data.v_debug_1[2] = (int)(BattSOC/10000); // SOC
        output_data.v_debug_1[3] = (int)(BattCon/10000); // Batt Con
    }
    
}//BDI_Meas end



/*===============================================[ private functions ]================================================*/



/*****************************************************************************
        Function:  BDI_Display
     Description:  converts the batt volts to % charge and displays it
******************************************************************************/
static void BDI_Display(void)
{
    //S32 tmp_l;

    if(BDI_run && !BDI_PEDtest)
    {
        S32 tmp_l = 0;

        // first predict actual state of charge (SOC)
        tmp_l = BattSOC - Vempty;

        if(tmp_l > -1)
        {
            tmp_l /= ((vfull_cal - Vempty) / 100);
            if (tmp_l > 100) 
            {
                tmp_l=100 ;  // catch overcharge to stop wrapping
            }
            BDI_BattCharge = tmp_l; //BattCharge is now a %
        }
        else
        {
            BDI_BattCharge = 0;
        }

        // now the current battery loading (CBL)
        tmp_l = BattCon - Vempty;

        if(tmp_l > -1 )
        {
            tmp_l /= ((vfull_cal - Vempty)/100);
            if(tmp_l > 100)
            {
                tmp_l = 100 ;  // catch overcharge to stop wrapping
            }
            BDI_BattCon = tmp_l;
        }
        else
        {
            BDI_BattCon = 0;
        }

        // sort out segment so CBL is never greater than SOC

        if(((BDI_BattCon) / 10)>((BDI_BattCharge) / 10))
        {
            BDI_BattCon = BDI_BattCharge;
        }


        // Battery discharge warnings
        if(BDI_BattCharge < 2)
        {
            BDI_LowBattFlag = 1;
        }
        else 
        {
            BDI_LowBattFlag = 0;
        }
        if(BDI_BattCharge < 1)
        {
            BDI_BattCutFlag = 1;
        }
        else
        {
            BDI_BattCutFlag = 0;
        }
    }// end if(BDI_go)
   //Dg_BattCharge = (int)(service->bdi_battcharge & 0x7f) ;
   
   output_data.ba_qbat_Disp = BDI_BattCharge;       /* send message to output buffer */
}//BDI_Display end

/*****************************************************************************
        Function:  BDI_Model
     Description:  This is the clever bit
******************************************************************************/
static void BDI_Model(void)
{
	#if 0
	S32 kk0;
	S32 kk1;
	S32 a;
	S32 b;
	S32 d;
	#endif

    // Qmon moved to here 01.07.04 ROJ
    if(Vin > Qmax)
    {
        Qmax = Vin;
        Qmax_cnt = Qmon_cnt;
    }
    else
    {
        if(Vin > Qmax_next)
        {
            Qmax_next = Vin;
            Qmax_next_cnt = Qmon_cnt;
        }
    }

    if(Vin < Qmin)
    {
        Qmin = Vin;
        Qmin_cnt = Qmon_cnt;
    }
    else
    {
        if(Vin < Qmin_next)
        {
            Qmin_next = Vin;
            Qmin_next_cnt = Qmon_cnt;
        }
    }

    if(Qmon_cnt > QTIME) 
    {
        Qmon_cnt = 0;
    }
    else  
    {
        Qmon_cnt++;
    }

    if(Qmon_cnt == Qmax_cnt)
    {
        Qmax = Qmax_next;
        Qmax_cnt = Qmon_cnt;
        Qmax_next_cnt = Qmax_next = 0;
    }

    if(Qmon_cnt == Qmin_cnt)
    {
        Qmin = Qmin_next;
        Qmin_cnt = Qmon_cnt;
        Qmin_next_cnt = Qmin_next = 25000;
    }

    if(bdicnt < 2)    // Initial value calculation from battery volts
    {
        BattSOC = (S32)Vin*10000;
        if((ABS(BattSOC - stReadWriteParam.pre_soc) < 680000) && (BattSOC < VfullRst))
        {
            BattSOC = stReadWriteParam.pre_soc;
        }
        BattCon = BattSOC;
        bdicnt++; // should be good data so display it
    }
    else
    {
		#if 1
		S32 kk0;
		S32 kk1;
		S32 a;
		S32 b;
		S32 d;
		#endif

        BDI_run = 1;                 // start BDI shortly after powerup to overcome random data at startup

        last_Batt_In = Batt_In;      // store for recovery correction for BattCon
 
        Batt_In = (S32)Vin * 10000;   // Rescale to allow small inc/dec for model

        //--------------- SOC ---------------
        a = 0;  // if batt_in  > battSoc, a should be 0, otherwise if BattSOC > 2.14, a = 2
        b = 0;  // if b is in normal range, this is 0
        d = 0;

        if( (Batt_In - BattSOC) >=0)
        {
            kk1 = k1;
            kk0 = k0; // 25000000=0.25V
        }
        else
        {
            if(BattSOC > vfull_cal) //2.14V
            {
                a += 2; // Vin < SOC && SOC > 2.14v per cell a += k3, decrese faster
            }

            kk1 = k1/k2;
            kk0 = 0;
        }


        //----- Quiescent Voltage monitor & error correction -------
        // This is used to correct errors in the SOC model

        if(BattSOC > Batt_In)
        {
            if((Qmin < 25000) && (Qmax > 0))// only after first value 2.5V
            {
                if((Qmin + 100) >= Qmax)// (Qmax - Qmin) < 0.01V
                {
                    a += 4;
                    d = 8;//set BattCon condition here
                }
            }
        }
        
        if (Vin > 24000) // 2.4V  2.4*24 = 57.6V
        {
            b += 5; // Vin > Vgas a = a + k7     ... Gassing
        }
        else if(Vin > 21800)//2.18V BAT 2.18*24=52.32
        {
            b += 1; // Vin > Vhi  a = a + k6     ... Full Charge
        }
        else if(Vin < 15500) // 1.55V 1.55*24=37.2
        {
            b += 1;            // internal resistance factor
        }
        else  
        {
            b =  0;
        }

        //---------------------------------------
        // full charge  need to (218)
        BattSOC += ((Batt_In - BattSOC + kk0) * (1 + a))/((1 + b) * (kk1));

        //---------------------------------------------------------

        // ---- BattCon ----
        if( (Batt_In - BattCon) >=0)
        {
            kk1 = k1;
            kk0 = k0;
        }
        else
        {
            kk1 = 13333;
            kk0 = 0;
        }

        a = 0;

        if (Vin < 12000)
        {
            a += 9;
        }
        if (Vin < 13000)
        {
            a += 5;
        }
        if (Vin < 14000)
        {
            a += 3;
        }
        if (Vin < 15000)
        {
            a += 1;
        }

        // BattCon = BattCon + ((((Batt_In - BattCon)*(1+c))+(kk0*k8)+d)/kk1)
        BattCon += (((Batt_In - BattCon) * (1 + a) + (kk0 * 2) + d)/kk1);
 
        if(BattCon > BattSOC)  
        {
            BattCon=BattSOC;   // BattCon can never be greater than BattSOC if BattSOC > 1.93 V per cell!!!
        }
        if(BattCon < 193000000)
        {
            BattCon=193000000; // impose 1.93 volt limit on BattCon
        }
        //RTC_WriteBackupRegister(RTC_BKP_DR11, BattSOC);
        RC ret = sd_write( NVRAM_SD,&(pReadWriteparam->pre_soc),  &BattSOC,sizeof(BattSOC) );
		if(RC_SUCCESS != ret)
		{
			/* Misra-c */
		}
        stReadWriteParam.pre_soc = BattSOC;
    }
}// end BDI_Model
//---------------------------------------------------------------------------

//************************************************************//
// Name: HrsRem(int Charge)
// Inputs: Battery charge calculated from measured voltage
// Description:
// Performs a prediction and smoothing algorithm on the battery
// charge to give an estimate of how many hours the battery
// could continue at the current load.
// In order to smooth the transient behaviour (which is very
// spikey) a second order Butterworth filter is applied. The
// filter has been transformed from the s-domain into the
// z-domain using the Bilinear Transform and the coefficients
// have been precalculated for a sample time of 10s and a
// corner frequency of 0.0005rad/s (approx. 1.8 rad/hr or a
// period of 318s or 32 samples)
//
// PT - 05/12/07
//************************************************************//
// see "Battery Hours Remaining.xls:Final Full (integer)" for an explanation of this algorithm

#if 0
void HrsRem(int Charge)
{
    static unsigned int     tDiff   = 0;            // time differnce between changes in charge
    static unsigned int     tInc    = 0;            // count the 250ms increments until 10s has passed

    static unsigned int     Ch_k    = 0;            // current charge
    unsigned  int           Ch_km1  = 0;            // previous charge
    signed int              ChGrad  = 0;            // Difference between current charge and previous charge

    unsigned int            Restart = 0;            // Restart state: 0 = normal operation, 1 = restarting this time
    static unsigned int     NGIY_flag = 0;          // no-gradient-info-yet flag:   0 = normal operation
                                                    //                          1 = have restarted and only received one value
                                                    //                          2 = have just detected the first gradient after a restart

    static signed long      InitPred_k     = 0;     // current initial prediction
    static signed long      InitPred_km1   = 0;     // previous initial prediction
    static signed long      PredMod_k      = 0;     // current modified prediction
    static signed long      PredMod_km1    = 0;     // previous modified prediction
    static signed long      PredMod_km2    = 0;     // previous previous modified prediction
    static signed long      ButterPred_k   = 0;     // Butterworth current seconds remaining prediction
    static signed long      ButterPred_km1 = 0;     // Butterworth previous seconds remaining prediction
    static signed long      ButterPred_km2 = 0;     // Butterworth previous previous seconds remaining prediction

    if ((tInc == 0) && (input_data.seat_switch_1 < 0))      // 10s has elapsed and seat is occupied (no calculations when truck not in use to prevent hours increasing)
    {
        tDiff++;                                    // increment the amount of elapsed time [10s]

        Ch_km1 = Ch_k;                              // update historical value of charge
        Ch_k = Charge;                              // new value of charge
        // if the charge is close to zero and the previous charge was high, it's probably an erroneous zero
        if ((Ch_k < ChIsZeroThreshold) && (Ch_km1 > ChIsHighThreshold))
        {
            Ch_k = Ch_km1;
        }

        // Look for a change in charge
        ChGrad = (signed int) Ch_km1 - (signed int) Ch_k;   // reduction is positive so -(k - km1) is written as (km1 - k)

        // Restart State
        Restart = (ChGrad < (- (signed int) ButterRestartGradient));

        // Initial Prediction Calculation
        // Excel: InitPred_km1 = IF(Restart, (Ch_k/100*Ch100pc), InitPred_k)
        if (Restart)                                // a restart has occured
        {
            InitPred_km1 = Ch_k * Ch100pc / 100;      // set the prediction to be a proportion of 8hrs
        }
        else
        {
            InitPred_km1 = InitPred_k;              // carry the historical value
        }
        // Excel: InitPred_k = IF(Restart, (Ch_k/100*Ch100pc),IF(ChGrad>0, td[s]/ChGrad*Ch_k, InitPred_km1))
        if (Restart)
        {
            NGIY_flag = 1;                          // set the no-gradient-info-yet flag
            InitPred_k = Ch_k * Ch100pc / 100;
        }
        else
        {
            if (ChGrad > 0)
            {   // a valid percentage reduction has occured
                if (NGIY_flag) 
                {
                    NGIY_flag = 2;       // set the no-gradient-info-yet flag to indicate it's about to be cleared
                }
                InitPred_k = tDiff*10 * Ch_k/ChGrad;
            }
            else
            {
                InitPred_k = InitPred_km1;          // a percentage rise or no change has occured, so keep the old value
            }
        }


        // Prediction Modification
        if (!Restart)
        {   // for normal operation, these history values update normally
            PredMod_km2 = PredMod_km1;
            PredMod_km1 = PredMod_k;
        }
        // Excel: PredMod_k = IF(InitPred_k=0, PredMod_km1, MIN(InitPred_k, Ch100pc))
        if (InitPred_k == 0)
        {
            PredMod_k = PredMod_km1;                // use previous value instead
        }
        else
        {
            if (InitPred_k > Ch100pc)
            {
                PredMod_k = Ch100pc;                // cap the value at 8hrs
            }
            else
            {
                PredMod_k = InitPred_k;             // pass value unmodified
            }
        }
        if (Restart)
        {   // however, for a restart, both history values are set to be equal to the current value
            PredMod_km2 = PredMod_k;
            PredMod_km1 = PredMod_k;
        }


        // Apply 2nd Order Butterworth filter (depending on starting conditions)
        if (!Restart)
        {   // for normal operation, these history values update normally
            ButterPred_km2 = ButterPred_km1;
            ButterPred_km1 = ButterPred_k;
        }
        else
        {   // however, for a restart, both history values are set to be equal to the current prediction value
            ButterPred_km2 = PredMod_k;
            ButterPred_km1 = PredMod_k;
        }

        if (NGIY_flag == 2)
        {   // this flag is 2 only if the charge is in the transition from having no gradient after a restart, to having one
            ButterPred_km2 = PredMod_k;
            ButterPred_km1 = PredMod_k;
            NGIY_flag = 0;     // immediately set back to 0 after getting a 2; 2 is a transitional value only
        }

        if (NGIY_flag == 1)
        {
            BDI_HrsRem = -1;  // hours remaining is undefined
        }
        else
        {       //            c_u(k) x  u(k)       c_u(k-1) x  u(k-1)       c_u(k-2) x  u(k-2)                       c_y(k-1) x  y(k-1)          c_y(k-2) x  y(k-2)
            ButterPred_k = (Coeff_uk*PredMod_k + Coeff_ukm1*PredMod_km1 + Coeff_ukm2*PredMod_km2) / CoeffScale1 + (Coeff_ykm1*ButterPred_km1 + Coeff_ykm2*ButterPred_km2) / CoeffScale2;
            BDI_HrsRem = ButterPred_k/360;          // convert seconds to hr/10, i.e. 1/3600*10
        }

        // update time variable
        if (ChGrad != 0)
        {
            tDiff = 0;
        }

        // the five things below are all displayed on the pedtrm "BDI" screen
        //BDI_VBatt  = ;
        //BDI_BattCharge = ;
        cntBattHrSamples = BDI_HrsRem;
        BattTimer = tDiff*10;
        //BDI_HrsRem = ;
    }

    tInc++;
    if (tInc >= 50)                             // 10s/200ms = 50 - this must be 50 because the sampletime is hardcoded into the
    {
        tInc = 0;                               // Butterworth filter coefficients as 10s - see the #defines above the function
    }
}
#endif


/***********************************************************************************************************************
*                             Copyright 2016 Linde Material Handling. All rights reserved.                             *
***********************************************************************************************************************/


